/**************************************************************/
/*  This file is licensed from, and is a trade secret of:     */
/*                                                            */
/*    Mercury Computer Systems, Incorporated                  */
/*    199 Riverneck Road                                      */
/*    Chelmsford, Massachusetts 01824-2820                    */
/*    United States of America                                */
/*    Telephone + 1 978 256-1300                              */
/*                                                            */
/*  Refer to your Software License Agreement for provisions   */
/*  on use, duplication, or disclosure.                       */
/*  US. Government users and prime contractors are also       */
/*  governed by the following Data Rights Notice:             */
/*  Use, duplication or disclosure by the US. Government is   */
/*  subject to the provisions set forth in FAR 52.227-14 for  */
/*  restricted rights or other agency data rights provisions  */
/*  on commercial software and documentation, as applicable.  */
/*                                                            */
/*  This file: (a) was developed at private expense, and no   */
/*  part was developed with government funds, (b) is a        */
/*  trade secret of Mercury Computer Systems, Inc. for the    */
/*  purposes of the Freedom of Information Act, (c) is        */
/*  "commercial computer software" subject to limited         */
/*  utilization as provided in the above noted Software       */
/*  License Agreement and (d) is in all respects data         */
/*  belonging exclusively to Mercury Computer Systems, Inc..  */
/*                                                            */
/**************************************************************/

/******************************************************************/
/* Copyright (c) 1999-2003, Mercury Computer Systems, Inc.,       */
/* Chelmsford MA.,and all third party embedded software sources.  */
/* All rights reserved under the Copyright laws of US. and        */
/* international treaties.                                        */
/*                                                                */
/* Patent Pending                                                 */
/******************************************************************/


//$COPYRIGHT_END


#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "msk.h"

using namespace std;

extern "C" {
  MOD_EXPORT
  POA_CF::Resource *
  CreateMskDemodulator (const char * identifier)
  {
    MskDemodulator * Demodulator;

    if (MskDemodulator::Create (Demodulator, identifier) != success) {
      return 0;
    }

    return (POA_CF::Resource *) Demodulator;
  }
}

status
MskDemodulator::Create (MskDemodulator *& handle,
                        const char * identifier)
{
  MskDemodulator *Temp = new MskDemodulator (identifier);

  if (!Temp) {
    handle = 0;
    return fail;
  }

  handle = Temp;
  return success;
}


MskDemodulator::MskDemodulator (const char * identifier)
  : CF__Resource_impl (identifier),
    m_threadrunning (false),
    m_bStop (false)
{
  m_uiSamplesPerSymbol = 4;  // must be at least 2 for MSK
  m_uiMaxVal = 32767;
  m_uiLenPulseShape = m_uiSamplesPerSymbol;
  m_j_pulse_shape = 0;
  m_mod_syms = 0;

  m_input_buffer_size = 4096;

  // define ports
  m_complexin = new MskInPort_impl (m_input_buffer_size);
  m_demod_syms = new char [FrameSize * m_uiSamplesPerSymbol / 2];
  m_fm3trout = new Fm3trOutPort_impl (m_demod_syms, FrameSize);
}

MskDemodulator::~MskDemodulator ()
{
  delete m_complexin;
  delete m_fm3trout;
  delete [] m_demod_syms;
  delete [] m_mod_syms;
  delete [] m_j_pulse_shape;
}

CORBA::Object_ptr
MskDemodulator::getPort (const CORBA::Char * name)
  throw (CORBA::SystemException,
	 CF::PortSupplier::UnknownPort)
{
  string temp(name);

  if (temp == "MSK_DemodulatorIn") {
    return m_complexin->getPort();
  }
  else if (temp == "MSK_DemodulatorOut") {
    return m_fm3trout->getPort();
  }
  else {
    throw CF::PortSupplier::UnknownPort();
  }
}  // MskModulator::getPort()

void
MskDemodulator::start()
  throw (CORBA::SystemException, CF::Resource::StartError)
{
  if (m_threadrunning) {
    return;
  }

  m_threadrunning = true;
  m_thread.start (RunWorker, (void *) this);
  CF__Resource_impl::start();
} // MskModulator::start()

void
MskDemodulator::stop ()
  throw (CORBA::SystemException,
	 CF::Resource::StopError)
{
  m_bStop = true;
  m_threadrunning = false;
  m_thread.detach();
} // MskModulator::stop()


status
MskDemodulator::Init ()
{
#ifdef MCS_VERBOSE
  std::cout << "MskDemodulator: Initializing." << std::endl;
#endif

  /* Reserve storage. */
  if (!m_mod_syms) {
    m_mod_syms = new CMPLX_INT16 [FrameSize];
  }

  /* Make sure pulse shape values will fit into a 16 bit signed integer. */
  if (m_uiMaxVal > 32767) {
    return fail;
  }

  // Generate pulse shape array.
  m_j_pulse_shape = new short [m_uiLenPulseShape];

  if (!m_j_pulse_shape) {
    return fail;
  }

  float t = static_cast<float>(PI/m_uiSamplesPerSymbol);
  m_pulseEnergy = 0.0f;
  for (unsigned i=0; i<m_uiSamplesPerSymbol; i++) {
    m_j_pulse_shape[i] = (short) ((float)128*sin((double)(i+1)*t));
    m_pulseEnergy += m_j_pulse_shape[i] * m_j_pulse_shape[i];
  }

  m_pulseEnergy /= (float) sqrt ((double) m_pulseEnergy);
  return success;
}

// Generic thread OS signature which calls component specific thread function
void
MskDemodulator::RunWorker (void * param)
{
  MskDemodulator * component = (MskDemodulator *) param;

#ifdef MCS_VERBOSE
  std::cout << "MskDemodulator: Component is running." << std::endl;
#endif

  component->RunLoop ();

#ifdef MCS_VERBOSE
  std::cout << "MskDemodulator: Component is stopping." << std::endl;
#endif
}

// Inner loop function that checks for a stop command
void
MskDemodulator::RunLoop ()
{
  m_bStop = false;

  // initialize run conditions
  Init ();

  while (!m_bStop) {
    Run ();
  }

  m_complexin->ReleaseDataBuffer ();
}

/*****************************************************************************
 * Function: void MskDemodulator::Demodulate (
 *	CMPLX_INT16 * data,
 *	unsigned & len_data,
 *	char * demod_syms,
 *	unsigned & num_syms,
 *	int scale)
 * 
 * Description: Demodulates a MSK modulated waveform at baseband and 
 *	generates an array of symbols (soft or hard, depending
 *	upon preprocessor definitions).
 *   
 * Operation:
 *   Inputs:
 *     data - complex baseband data from receiver
 *     len_data - length of data array
 *     scale - scaling value used for converting int to float
 *
 *   Outputs:
 *     demod_syms - demodulated soft decision symbols
 *     len_data - actual number of elements read from data array
 *     num_syms - number of symbols returned
 *
 * Notes/Issues:
 *
 *****************************************************************************/

void
MskDemodulator::Demodulate (CMPLX_INT16 * data,
                            unsigned & len_data,
                            char * demod_syms,
                            unsigned & num_syms,
                            int scale)
{
  // Calculate delay.
  unsigned delay = 0;

  /*
   * No need to calculate delay if waveform is running on a machine and not
   * in the analog domain.
   */

  data += delay;

  /*
   * Use matched filter with float calculations.  Assume delay is correct.
   * Return soft decision; integer value between -127 and 127.
   */

  unsigned p=0, n, k=0;
  unsigned half_pulse_length = m_uiSamplesPerSymbol/2;
  short sample_I;
  short sample_Q;
  num_syms = 0;

  float symbolEnergy_I, symbolEnergy_Q;
  float xcorr_I, xcorr_Q;

  for (p=0; p<len_data; p+=m_uiSamplesPerSymbol) {
    symbolEnergy_I = 0.0f;
    symbolEnergy_Q = 0.0f;
    xcorr_I = 0.0f;
    xcorr_Q = 0.0f;

    for (n=0; n<m_uiSamplesPerSymbol; n++) {
      sample_I = data[p+n+delay].real ();
      if (p+n+delay+half_pulse_length < len_data) {
	sample_Q = data[p+n+delay+half_pulse_length].imag ();
      }
      else {
	sample_Q = 0;
      }

      // Update both I and Q symbol energy variables.
      symbolEnergy_I += (float) sample_I * (float) sample_I;
      symbolEnergy_Q += (float) sample_Q * (float) sample_Q;

      // Correlate received pulse with reference pulse shape.
      xcorr_I += (float) sample_I *(float) m_j_pulse_shape[n];
      xcorr_Q += (float) sample_Q *(float) m_j_pulse_shape[n];
    }
    // calculate symbol energies
    symbolEnergy_I = (float) sqrt ((double) symbolEnergy_I);
    symbolEnergy_Q = (float) sqrt ((double) symbolEnergy_Q);

    // normalize correlation variables.
    // should be a number between -1 and 1
    xcorr_I /= (float) (symbolEnergy_I * m_pulseEnergy);
    xcorr_Q /= (float) (symbolEnergy_Q * m_pulseEnergy);

    // Return soft decision.  Perfect correlation is +/- 127
    demod_syms[k++] = (char) ( xcorr_I * 127 );
    demod_syms[k++] = (char) ( xcorr_Q * 127 );
    num_syms += 2;
  }

  // return number of input samples that were actually read
  len_data = num_syms*m_uiSamplesPerSymbol/2 + delay;
}

unsigned
MskDemodulator::SymbolSynch (CMPLX_INT16 * data,
                             unsigned len_data,
                             int scale)
{
  /*
   * Finds the delay in the received signal based upon the first few
   * received symbols.
   */

  unsigned m = 4; // number of symbols over which to correlate
  unsigned k = m_uiSamplesPerSymbol; // for code brevity
  unsigned half_pulse_length = m_uiSamplesPerSymbol/2;

  if ( (m+1)*k - 1 > len_data ) {
    // Not enough data to compute delay!
    return 0;
  }

  // initialize correlation vector
  CMPLX_INT16 * x_corr = new CMPLX_INT16 [m*k];

  unsigned delay = 0, index_I, index_Q;
  short sample_I, sample_Q, xcorr_I, xcorr_Q;
  short * delay_vect = new short[k];
  unsigned i, j; // indices

  // Correlate the reference pulse with the first "m" symbols.

  for (i=0; i<m*k; i++) {
    // initialize correlation values
    xcorr_I = 0;
    xcorr_Q = 0;

    // perform correlation
    for (j=0; j<k; j++) {
      // Initialize indices; Q is delayed by half a pulse length.
      index_I = (i+j) % (m*k);
      index_Q = (i+j+half_pulse_length) % (m*k);

      // Read sample and scale to prevent integer overflow.
      sample_I = data[index_I].real() / (short) scale;
      sample_Q = data[index_Q].imag() / (short) scale;

      // Accumulate.
      xcorr_I += sample_I * (m_j_pulse_shape[j] / k);
      xcorr_Q += sample_Q * (m_j_pulse_shape[j] / k);
    }

    x_corr[i] = CMPLX_INT16 (abs(xcorr_I), abs(xcorr_Q));
  }

  // Find the maximum cross-correlation index by averaging over "m" symbols.
  short max_rxy = -1;
  for (j=0; j<k; j++) {
    delay_vect[j] = 0;

    for (i=0; i<(m*k); i+=k) {
      delay_vect[j] += ( x_corr[j+i].real()+x_corr[j+i].imag() ) / (k*2) ;
    }

    if (delay_vect[j] > max_rxy) {
      max_rxy = delay_vect[j];
      delay = j;
    }
  }

  return delay;
}

// Inner loop method that performs component business logic
status
MskDemodulator::Run ()
{
  int intotal=0, outtotal=0;
  unsigned int ii, count;

#ifdef MCS_VERBOSE
  int j=0;  // counter for packetizing input stream
#endif

  intotal = 0;
  outtotal = 0;

  m_input_count = m_complexin->GetData(&m_input);

  if (m_input_count == 0) {
    return fail;
  }

  m_samples_read = 0;

  while ((unsigned) m_input_count > m_uiSamplesPerSymbol ) {
    // Cap number of symbols read at the buffer size (FrameSize).
    count = (m_input_count > FrameSize) ? FrameSize : m_input_count;

    // Read data from input buffer into m_mod_syms array for demodulation.
    for (ii=0; ii<count; ii++) {
      m_mod_syms[ii] = m_input[ii];
    }

    int scale = 256;
    unsigned num_demod_syms;
    Demodulate (m_mod_syms, count, m_demod_syms, num_demod_syms, scale);

    /*
     * Count may have changed with demodulator, depending on symbol
     * synchronization.
     */

    m_fm3trout->pushData (m_demod_syms, num_demod_syms);
    m_samples_read += count;
    m_input_count -= count;
    m_input += count;

#ifdef MCS_VERBOSE
    std::cout << "MskDemodulator: Demodulating packet " << j++
	      << " " << count << "/" << count/m_uiSamplesPerSymbol
	      << std::endl;
#endif

    intotal += count;
    outtotal += count/m_uiSamplesPerSymbol;
  }

  /*
   * Release buffer when there are no more data to be read.  There may,
   * however, be some residual samples left over because only even
   * multiples of samples per symbol are read at a time.
   */

  m_complexin->ReleaseDataBuffer(m_samples_read);

#ifdef MCS_VERBOSE
  std::cout << "MskDemodulator: At EOD, total " 
	    << intotal << "/" << outtotal << " values"
	    << std::endl;
#endif

  return success;
}
